#include <cstdlib>
#include <ros/ros.h>
#include <glog/logging.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>

typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
int main(int argc, char **argv)
{
    ros::init(argc, argv, "simple_goal");
    float x = 0, y = 0, yaw = 0;
    // auto nh = std::make_shared<ros::NodeHandle>("~");
    // nh->param<float>("position_x", x);
    // nh->param<float>("position_y", y);
    // nh->param<float>("angular_yaw", yaw);
    if (argc == 4) {
        x = std::stof(argv[1]);
        y = std::stof(argv[2]);
        yaw = std::stof(argv[3]);
    }

    MoveBaseClient ac("move_base", true);
    while (!ac.waitForServer(ros::Duration(5.0))) {
        LOG(INFO) << "[simple_goal] Waiting for the move_base action server to come up";
        // ROS_INFO("Waiting for the move_base action server to come up");
    }
    
    move_base_msgs::MoveBaseGoal goal;
    goal.target_pose.header.frame_id = "base_link";
    goal.target_pose.header.stamp = ros::Time::now();
    goal.target_pose.pose.position.x = x;
    goal.target_pose.pose.position.y = y;
    goal.target_pose.pose.orientation.z = std::sin(yaw * 0.5);
    goal.target_pose.pose.orientation.w = std::cos(yaw * 0.5);
    
    LOG(INFO) << "[simple_goal] ending goal";
    // ROS_INFO("Sending goal");
    ac.sendGoal(goal);
    ac.waitForResult();
    bool ret = EXIT_FAILURE;
    if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) {
        // LOG(INFO) << "[simple_goal] arrive";
        // ROS_INFO("arrive");
        ret = EXIT_FAILURE;
    }
    else {
        // LOG(INFO) << "[simple_goal] error";
        // ROS_INFO("error");
        ret = EXIT_SUCCESS;
    }
    // ros::spin();
    // ros::shutdown();
    return ret;
}